nop
nop
nop
nop
nop
nop
nop
nop
nop
nop
nop
nop
nop
nop
nop
nop
nop
nop
nop
nop
nop
nop
nop
####	Main	####
Main:
jal TurnOffInterrupts
###	find leds
addiu $a0, $zero, 0x01	#set category / brand to find
addiu $a1, $zero, 0x01	#set ID number
jal DRPFindDeviceSub
addu $s0, $a2, $zero	#save led address to s0

###  find seven segs
addiu $a1, $zero, 0x03	#set ID number
jal DRPFindDeviceSub
addu $s1, $a2, $zero	#save seg address to s1

### find keyboard
addiu $a1, $zero, 0x04	#set ID number
jal DRPFindDeviceSub
addu $s2, $a2, $zero	#save keyboard address to s2

### find servo
addiu $a0, $zero, 0x02	#set category / brand for pmod connectors
addiu $a1, $zero, 0x01
jal DRPFindDeviceSub
addu $s3, $a2, $zero	#save keyboard address to s3

### find stepper motor
addiu $a0, $zero, 0x05
addiu $a1, $zero, 0x01
jal DRPFindDeviceSub
addu $s4, $a2, $zero

beqz $s0, Main		#seven segs not supported just loop over this for ever
addu $t0, $zero, 0x01
sw $t0, ($s0)

beqz $s1, Main
addu $t0, $zero, 0x02
sw $t0, ($s0)

addu $a0, $zero, $s1
addiu $a1, $zero, 0xF
jal SevenSegEnable

addiu $a1, $zero, 0x0000
addiu $a2, $zero, 0xF
jal SevenSegDisplay

beqz $s2, Main
addu $t0, $zero, 0x03
sw $t0, ($s0)

beqz $s3, Main
addu $t0, $zero, 0x04
sw $t0, ($s0)

beqz $s4, Main
addu $t0, $zero, 0x05
sw $t0, ($s0)


##temp
lui $t0, 0xBE
ori $t0, $t0, 0xBC20
srl $t0, $t0, 3
sw $t0, 0x04($s4)
addiu $t0, $zero, 3
sw $t0, ($s4)


addu $s5, $zero, $zero
addu $s6, $zero, $zero
mainLoop:	addu  $a0, $s2, $zero
		jal KeyboardWGSUB
		lui $t1, 0x0100
		ori $t2, $t1, 0xE074 ## right arrow scan code
		beq $t2, $a1, rightPressed
		ori $t2, $t1, 0xE06B ## left arrow scan code
		beq $t2, $a1, leftPressed
		ori $t2, $t1, 0xE075 ### up arrow scan code
		beq $t2, $a1, forward
		ori $t2, $t1, 0xE072 ## down arrow scan code
		beq $t2, $a1, backward
		lui $t1, 0x03E0
		or $t2, $t1, 0xF075
		beq $t2, $a1, stop
		or $t2, $t1, 0xF072
		beq $t2, $a1, stop
before:		j mainLoop

rightPressed:
	addiu $s5, $s5, 1
	sw $s5, ($s0)
	addu $a0, $zero, $s3
	addu $a1, $zero, $s5
	jal ServoSet
	j before
leftPressed:
	subiu $s5, $s5, 1
	sw $s5, ($s0)
	addu $a0, $zero, $s3
	addu $a1, $zero, $s5
	jal ServoSet
	j before

forward:	addi $s6, $s6, 40
		addu $a0, $zero, $s4
		addu $a1, $zero, $s6
		jal stepper
		j mainLoop

backward:	subi $s6, $s6, 40
		addu $a0, $zero, $s4
		addu $a1, $zero, $s6
		jal stepper
		j mainLoop

stop:		addu $s6, $zero, $zero
		addu $a0, $zero, $s4
		addu $a1, $zero, $s6
		jal stepper
		j mainLoop
		
		
		
stepper:	bnez $a1, stepperOn
		sw $zero, ($a0)
		jr $ra
stepperOn:	addiu $t0, $zero, 1
		addu $t1, $zero, $a1
		bgtz $a1, stepperPos
		addiu $t0, $zero, 3
		addi  $t1, $zero, -1
		xor $t1, $a1, $t1
		addiu $t1, $t1, 1
stepperPos:	sll $t1, $t1, 23
		sw $t1, 4($a1)
		sw $t0, 0($a0)
		jr $ra
		
		lui $t0, 0xBE
		ori $t0, $t0, 0xBC20
		sw $t0, 0x04($s4)
		addiu $t0, $zero, 7
		sw $t0, ($s4)
		
stepperWriteM:	sw $t0, ($a0)
		jr $ra
 
	


##################
### step(void * stepper, int speed)
### negative speed is backwards
### speed is doubled to give full 32 bit range
### uses $t0, $t1
### $a0 = &stepper
### $a1 = signed_speed
step:	bne $a1, $0, step_0      # if (signed_speed == 0)      // if 'off'
	sw $0, 0($a0)            #     stepper.status = 0;     // turn off
	jr $ra
step_0:	srl $t1, $a1, 31         # mDir = signed_speed[31];    // get direction bit
	ori $t0, $0, 1           # mStatus = 1;                // forwards, on
	sll $t1, $t1, 1          # mDir <<= 1;                 // get direction mask
	or $t0, $t0, $t1         # mStatus |= mDir;            // set direction in temp_status
	sw $t0, 0($a0)           # stepper.status = mStatus;   // set status

	beq $t1, $0, step_1      # if (mDir != 0)              // if backwards
        sub $t0, $0, $a1         #     mSpeed = -signed_speed; // get positive speed

step_1:	sll $t0, $t0, 1          # mSpeed *= 2;                // to use the full range
	sw $t0, 4($a0)           # stepper.speed = mSpeed;     // set speed

	jr $ra




######### turn off interrupts
TurnOffInterrupts:	mfc0 $t0, $12
			andi $t0, $t0, -2 #turn off intterrupt enable bit
			mtc0 $t0, $12
			jr $ra
			
			
####	Device Number finder ####
#### a0 = device brand / category 
#### a1 = device number
#### a2 = device address
DRPFindDeviceSub:	lui $t0, 0xA000
			ori $t0, $t0, 0x000A
			and $t1, $a0, 0x00FF
			sll $t1, $t1, 16
			or $t0, $t1, $t0
			and $t1, $a1, 0x0FFF
			sll $t1, $t1, 4
			or $t0, $t1, $t0
			# we now have the DRP ID we are looking for
			lui $a2, 0xFFFF
			ori $t1, $a2, 0x3C	#t2 contains end address
					
DRPFindDeviceL:		lw $t2, 0x3C($a2)
			and $t2, $t0, $t2
			beq $t2, $t0, DRPFindDeviceFound	#jump if DRP matches
			beq $a2, $t1, DRPFindDeviceNFound	#checked last address and not found
			addiu $a2, $a2, 0x40
			j DRPFindDeviceL
			
DRPFindDeviceNFound:	add $a2, $zero, $zero
DRPFindDeviceFound:	jr $ra





			
			
#########################################
############ write to servo ############
### a0 = address of device

############## a1 = angle of servo
ServoSet:		addiu $t0, $a1, 10
			ori $t0, $t0, 0x80
			sll $t1, $t0, 8
			or $t0, $t0, $t1  #t0 contains 
			sll $t1, $t0, 16
			or $t0, $t1, $t0
			sw $t0, ($a0)
			jr $ra


#########################################
######### display 7 segs ################
####	a0 = address
###	a1 = numbers
###	a2 = dots

SevenSegDisplay:	andi $t0, $a2, 0x000F		#takes the 4 bits of dot argument
			ori $t0, $t0, 0x00F0		#enables write mask for all digits
			sll $t0, $t0, 16		#shifts bits up by 16 bits
			andi $t1, $a1, 0xFFFF		#takes the 16 bits of the number that are important
			or $t0, $t0, $t1		#add with 16 bits numbers to the rest of the value
			sw $t0, 0x4($a0)		#writes value to seven 7 controller
			jr $ra				#returns 
#########################################
########### enable 7 segs ###############
###### a0 = address
###### a1 = digits to enable
SevenSegEnable:		andi $t0, $a1, 0x000F		#takes the 4 bits of the digits
			lui $t1, 0xFFFF			#preping address in t1
			sw $t0, ($a0)		#writes value to seven 7 controller
			jr $ra
########################################
############ Wait & Get Keybaord ##############
####### a0 = keyboard address
####### a1 = result
KeyboardWGSUB:		lw $t0, ($a0)
			bnez $t0, KeyboardWGReady
			j KeyboardWGSUB
KeyboardWGReady:	lw $a1, 0x4($a0)
			jr $ra
			
			
###############################################
########### switches wait and get #############
SwitchesWG:		lui $t0, 0xFFFF
SwitchesWGC:		lw $t1, 0x40($t0)
			#sw $t0, 0x00($t0)
			andi $t2, $t1, 0x80	#check highest bit switch is on (simulating the keyboard key ready bit)
			xori $t3, $s6, 0x80	#invert enable bit in past value
			move $s6, $t2		#save last value
			and $t3, $t3, $t2	#the last three lines effectingly do (~pastValue & new value) this is to catch the rising edge of the switch 8
			#sw $t1, 0x00($t0)
			#srl $t2, $t2, 7
			bnez $t3, SwitchesReady
			#sw $t0, 0x00($t0)
			j SwitchesWGC
SwitchesReady:		andi $a3, $t1, 0x7F
			
			jr $ra

